
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_motors_matrix.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_motors_matrix.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/// Constructor
void MotorsMatrix(MotorsMat_HandleTypeDef *pMotor, uint16_t loop_rate, uint16_t speed_hz)
{
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    MotorsMulticopter(motors_mc, loop_rate, speed_hz);
}

void MotorsMat_set_armed(MotorsMat_HandleTypeDef *pMotor, bool arm)
{
    Motors_HandleTypeDef *motors = (Motors_HandleTypeDef *)pMotor;
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    if (motors->_armed != arm) {
        motors->_armed = arm;
        if (!arm) {
            MotorsMC_save_params_on_disarm(motors_mc);
        }
    }
}

// init
void MotorsMat_init(MotorsMat_HandleTypeDef *pMotor, motor_frame_class frame_class, motor_frame_type frame_type)
{
    Motors_HandleTypeDef *motors = (Motors_HandleTypeDef *)pMotor;

    // record requested frame class and type
    pMotor->_last_frame_class = frame_class;
    pMotor->_last_frame_type = frame_type;

    // setup the motors
    MotorsMat_setup_motors(pMotor, frame_class, frame_type);

    // enable fast channels or instant pwm
    MotorsMat_set_update_rate(pMotor, motors->_speed_hz);

}

// set update rate to motors - a value in hertz
// you must have setup_motors before calling this
void MotorsMat_set_update_rate(MotorsMat_HandleTypeDef *pMotor, uint16_t speed_hz)
{
    Motors_HandleTypeDef *motors = (Motors_HandleTypeDef *)pMotor;
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    // record requested speed
    motors->_speed_hz = speed_hz;

    pMotor->_motors_mask = 0;
    for (uint8_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            pMotor->_motors_mask |= 1U << i;
        }
    }

    //TODO：设置pwm输出频率
    Motors_rc_set_freq(motors, pMotor->_motors_mask, motors->_speed_hz);
}

// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
void MotorsMat_set_frame_class_and_type(MotorsMat_HandleTypeDef *pMotor, motor_frame_class frame_class, motor_frame_type frame_type)
{
    Motors_HandleTypeDef *motors = (Motors_HandleTypeDef *)pMotor;

    // exit immediately if armed or no change
    if (Motors_get_armed(motors) || (frame_class == pMotor->_last_frame_class && pMotor->_last_frame_type == frame_type)) {
        return;
    }

    MotorsMat_init(pMotor, frame_class, frame_type);
}

// output_test_seq - spin a motor at the pwm value specified
//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void MotorsMat_output_test_seq(MotorsMat_HandleTypeDef *pMotor, uint8_t motor_seq, int16_t pwm)
{
    Motors_HandleTypeDef *motors = (Motors_HandleTypeDef *)pMotor;
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    // exit immediately if not armed
    if (!Motors_get_armed(motors)) {
        return;
    }

    // loop through all the possible orders spinning any motors that match that description
    for (uint8_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i] && pMotor->_test_order[i] == motor_seq) {
            // turn on this motor
            Motors_rc_write(motors, i, pwm);
        }
    }
}

// output_test_num - spin a motor connected to the specified output channel
//  (should only be performed during testing)
//  If a motor output channel is remapped, the mapped channel is used.
//  Returns true if motor output is set, false otherwise
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
bool MotorsMat_output_test_num(MotorsMat_HandleTypeDef *pMotor, uint8_t output_channel, int16_t pwm)
{
    Motors_HandleTypeDef *motors = (Motors_HandleTypeDef *)pMotor;
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    if (!Motors_get_armed(motors)) {
        return false;
    }

    // Is channel in supported range?
    if (output_channel > GP_MOTORS_MAX_NUM_MOTORS - 1) {
        return false;
    }

    // Is motor enabled?
    if (!motors_mc->motor_enabled[output_channel]) {
        return false;
    }

    Motors_rc_write(motors, output_channel, pwm); // output
    return true;

}

// output - sends commands to the motors
void MotorsMat_output(MotorsMat_HandleTypeDef *pMotor)
{
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;
    
    // update throttle filter
    MotorsMC_update_throttle_filter(motors_mc);

    // run spool logic
    MotorsMC_output_logic(motors_mc);

    // calculate thrust
    MotorsMat_output_armed_stabilizing(pMotor);

    // TODO: apply any thrust compensation for the frame
    // 某些类型需要推力补偿

    // convert rpy_thrust values to pwm
    MotorsMat_output_to_motors(pMotor);

    // output any booster throttle
    MotorsMC_output_boost_throttle(motors_mc);

    // output raw roll/pitch/yaw/thrust
    MotorsMC_output_rpyt(motors_mc);
}

// output_min - sets servos to neutral point with motors stopped
void MotorsMat_output_min(MotorsMat_HandleTypeDef *pMotor)
{
    Motors_HandleTypeDef *motors = (Motors_HandleTypeDef *)pMotor;

    motors->_spool_desired = MOTOR_DESIRED_SHUT_DOWN;
    motors->_spool_state = MOTOR_SHUT_DOWN;

    MotorsMat_output(pMotor);
}

// output_to_motors - sends minimum values out to the motors
void MotorsMat_output_to_motors(MotorsMat_HandleTypeDef *pMotor)
{
    Motors_HandleTypeDef *motors = (Motors_HandleTypeDef *)pMotor;
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    int8_t i;

    switch (motors->_spool_state) {
        case MOTOR_SHUT_DOWN: {
            // no output
            for (i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motors_mc->motor_enabled[i]) {
                    motors_mc->_actuator[i] = 0.0f;
                }
            }
            break;
        }
        case MOTOR_GROUND_IDLE:
            // sends output to motors when armed but not flying
            for (i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motors_mc->motor_enabled[i]) {
                    MotorsMC_set_actuator_with_slew(motors_mc, &(motors_mc->_actuator[i]), MotorsMC_actuator_spin_up_to_ground_idle(motors_mc));
                }
            }
            break;
        case MOTOR_SPOOLING_UP:
        case MOTOR_THROTTLE_UNLIMITED:
        case MOTOR_SPOOLING_DOWN:
            // set motor output based on thrust requests
            for (i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motors_mc->motor_enabled[i]) {
                    MotorsMC_set_actuator_with_slew(motors_mc, &(motors_mc->_actuator[i]), MotorsMC_thrust_to_actuator(motors_mc, pMotor->_thrust_rpyt_out[i]));
                }
            }
            break;
    }

    // convert output to PWM and send to each motor
    for (i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            Motors_rc_write(motors, i, MotorsMC_output_to_pwm(motors_mc, motors_mc->_actuator[i]));
        }
    }

}

// output - sends commands to the motors
void MotorsMat_output_armed_stabilizing(MotorsMat_HandleTypeDef *pMotor)
{
    Motors_HandleTypeDef   *motors = (Motors_HandleTypeDef *)pMotor;
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    uint8_t i;                          // general purpose counter
    float   roll_thrust;                // roll thrust input value, +/- 1.0
    float   pitch_thrust;               // pitch thrust input value, +/- 1.0
    float   yaw_thrust;                 // yaw thrust input value, +/- 1.0
    float   throttle_thrust;            // throttle thrust input value, 0.0 - 1.0
    float   throttle_avg_max;           // throttle thrust average maximum value, 0.0 - 1.0
    float   throttle_thrust_max;        // throttle thrust maximum value, 0.0 - 1.0
    float   throttle_thrust_best_rpy;   // throttle providing maximum roll, pitch and yaw range without climbing
    float   rpy_scale = 1.0f;           // this is used to scale the roll, pitch and yaw to fit within the motor limits
    float   yaw_allowed = 1.0f;         // amount of yaw we can fit in
    float   thr_adj;                    // the difference between the pilot's desired throttle and throttle_thrust_best_rpy

    // apply voltage and air pressure compensation
    const float compensation_gain = MotorsMC_get_compensation_gain(motors_mc); // compensation for battery voltage and altitude
    roll_thrust = (motors->_roll_in + motors->_roll_in_ff) * compensation_gain;
    pitch_thrust = (motors->_pitch_in + motors->_pitch_in_ff) * compensation_gain;
    yaw_thrust = (motors->_yaw_in + motors->_yaw_in_ff) * compensation_gain;
    throttle_thrust = Motors_get_throttle(motors) * compensation_gain;
    throttle_avg_max = motors->_throttle_avg_max * compensation_gain;

    // If thrust boost is active then do not limit maximum thrust
    throttle_thrust_max = motors->_thrust_boost_ratio + (1.0f - motors->_thrust_boost_ratio) * motors_mc->_throttle_thrust_max * compensation_gain;

    // sanity check throttle is above zero and below current limited throttle
    if (throttle_thrust <= 0.0f) {
        throttle_thrust = 0.0f;
        motors->limit.throttle_lower = true;
    }
    if (throttle_thrust >= throttle_thrust_max) {
        throttle_thrust = throttle_thrust_max;
        motors->limit.throttle_upper = true;
    }

    // ensure that throttle_avg_max is between the input throttle and the maximum throttle
    throttle_avg_max = math_constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max);

    // calculate the highest allowed average thrust that will provide maximum control range
    throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);

    // calculate throttle that gives most possible room for yaw which is the lower of:
    //      1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
    //      2. the higher of:
    //            a) the pilot's throttle input
    //            b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
    //      Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
    //      Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
    //      We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
    //      We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low.  We favor reducing throttle instead of better yaw control because the pilot has commanded it

    // Under the motor lost condition we remove the highest motor output from our calculations and let that motor go greater than 1.0
    // To ensure control and maximum righting performance Hex and Octo have some optimal settings that should be used
    // Y6               : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
    // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
    // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75,  ATC_RAT_PIT_IMAX = 0.75,  ATC_RAT_YAW_IMAX = 0.375
    // Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
    // Hex              : MOT_YAW_HEADROOM = 0,   ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
    // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25,  ATC_RAT_PIT_IMAX = 0.25,  ATC_RAT_YAW_IMAX = 0.25
    // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5,   ATC_RAT_PIT_IMAX = 0.5,   ATC_RAT_YAW_IMAX = 0.25
    // Quads cannot make use of motor loss handling because it doesn't have enough degrees of freedom.

    // calculate amount of yaw we can fit into the throttle range
    // this is always equal to or less than the requested yaw from the pilot or rate controller
    float rp_low = 1.0f;    // lowest thrust value
    float rp_high = -1.0f;  // highest thrust value
    for (i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            // calculate the thrust outputs for roll and pitch
            pMotor->_thrust_rpyt_out[i] = roll_thrust * pMotor->_roll_factor[i] + pitch_thrust * pMotor->_pitch_factor[i];
            // record lowest roll + pitch command
            if (pMotor->_thrust_rpyt_out[i] < rp_low) {
                rp_low = pMotor->_thrust_rpyt_out[i];
            }
            // record highest roll + pitch command
            if (pMotor->_thrust_rpyt_out[i] > rp_high && (!motors->_thrust_boost || i != pMotor->_motor_lost_index)) {
                rp_high = pMotor->_thrust_rpyt_out[i];
            }

            // Check the maximum yaw control that can be used on this channel
            // Exclude any lost motors if thrust boost is enabled
            if (!math_flt_zero(pMotor->_yaw_factor[i]) && (!motors->_thrust_boost || i != pMotor->_motor_lost_index)){
                if (math_flt_positive(yaw_thrust * pMotor->_yaw_factor[i])) {
                    yaw_allowed = MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + pMotor->_thrust_rpyt_out[i]), 0.0f)/pMotor->_yaw_factor[i]));
                } else {
                    yaw_allowed = MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + pMotor->_thrust_rpyt_out[i], 0.0f)/pMotor->_yaw_factor[i]));
                }
            }
        }
    }

    // calculate the maximum yaw control that can be used
    // todo: make _yaw_headroom 0 to 1
    float yaw_allowed_min = (float)motors_mc->_yaw_headroom / 1000.0f;

    // increase yaw headroom to 50% if thrust boost enabled
    yaw_allowed_min = motors->_thrust_boost_ratio * 0.5f + (1.0f - motors->_thrust_boost_ratio) * yaw_allowed_min;

    // Let yaw access minimum amount of head room
    yaw_allowed = MAX(yaw_allowed, yaw_allowed_min);

    // Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
    if (motors->_thrust_boost && motors_mc->motor_enabled[pMotor->_motor_lost_index]) {
        // record highest roll + pitch command
        if (pMotor->_thrust_rpyt_out[pMotor->_motor_lost_index] > rp_high) {
            rp_high = motors->_thrust_boost_ratio * rp_high + (1.0f - motors->_thrust_boost_ratio) * pMotor->_thrust_rpyt_out[pMotor->_motor_lost_index];
        }

        // Check the maximum yaw control that can be used on this channel
        // Exclude any lost motors if thrust boost is enabled
        if (!math_flt_zero(pMotor->_yaw_factor[pMotor->_motor_lost_index])){
            if (math_flt_positive(yaw_thrust * pMotor->_yaw_factor[pMotor->_motor_lost_index])) {
                yaw_allowed = motors->_thrust_boost_ratio * yaw_allowed + (1.0f - motors->_thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + pMotor->_thrust_rpyt_out[pMotor->_motor_lost_index]), 0.0f)/pMotor->_yaw_factor[pMotor->_motor_lost_index]));
            } else {
                yaw_allowed = motors->_thrust_boost_ratio * yaw_allowed + (1.0f - motors->_thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + pMotor->_thrust_rpyt_out[pMotor->_motor_lost_index], 0.0f)/pMotor->_yaw_factor[pMotor->_motor_lost_index]));
            }
        }
    }

    if (fabsf(yaw_thrust) > yaw_allowed) {
        // not all commanded yaw can be used
        yaw_thrust = math_constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
        motors->limit.yaw = true;
    }

    // add yaw control to thrust outputs
    float rpy_low = 1.0f;   // lowest thrust value
    float rpy_high = -1.0f; // highest thrust value
    for (i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            pMotor->_thrust_rpyt_out[i] = pMotor->_thrust_rpyt_out[i] + yaw_thrust * pMotor->_yaw_factor[i];

            // record lowest roll + pitch + yaw command
            if (pMotor->_thrust_rpyt_out[i] < rpy_low) {
                rpy_low = pMotor->_thrust_rpyt_out[i];
            }
            // record highest roll + pitch + yaw command
            // Exclude any lost motors if thrust boost is enabled
            if (pMotor->_thrust_rpyt_out[i] > rpy_high && (!motors->_thrust_boost || i != pMotor->_motor_lost_index)) {
                rpy_high = pMotor->_thrust_rpyt_out[i];
            }
        }
    }
    // Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
    if (motors->_thrust_boost) {
        // record highest roll + pitch + yaw command
        if (pMotor->_thrust_rpyt_out[pMotor->_motor_lost_index] > rpy_high && motors_mc->motor_enabled[pMotor->_motor_lost_index]) {
            rpy_high = motors->_thrust_boost_ratio * rpy_high + (1.0f - motors->_thrust_boost_ratio) * pMotor->_thrust_rpyt_out[pMotor->_motor_lost_index];
        }
    }

    // calculate any scaling needed to make the combined thrust outputs fit within the output range
    if (rpy_high - rpy_low > 1.0f) {
        rpy_scale = 1.0f / (rpy_high - rpy_low);
    }
    if (throttle_avg_max + rpy_low < 0.0f) {
        rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
    }

    // calculate how close the motors can come to the desired throttle
    rpy_high *= rpy_scale;
    rpy_low *= rpy_scale;
    throttle_thrust_best_rpy = -rpy_low;
    thr_adj = throttle_thrust - throttle_thrust_best_rpy;
    if (rpy_scale < 1.0f) {
        // Full range is being used by roll, pitch, and yaw.
        motors->limit.roll = true;
        motors->limit.pitch = true;
        motors->limit.yaw = true;
        if (thr_adj > 0.0f) {
            motors->limit.throttle_upper = true;
        }
        thr_adj = 0.0f;
    } else {
        if (thr_adj < 0.0f) {
            // Throttle can't be reduced to desired value
            // todo: add lower limit flag and ensure it is handled correctly in altitude controller
            thr_adj = 0.0f;
        } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy + rpy_high)) {
            // Throttle can't be increased to desired value
            thr_adj = 1.0f - (throttle_thrust_best_rpy + rpy_high);
            motors->limit.throttle_upper = true;
        }
    }

    // add scaled roll, pitch, constrained yaw and throttle for each motor
    for (i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            pMotor->_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * pMotor->_thrust_rpyt_out[i]);
        }
    }

    // determine throttle thrust for harmonic notch
    const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
    // compensation_gain can never be zero
    motors->_throttle_out = throttle_thrust_best_plus_adj / compensation_gain;

    // check for failed motor
    MotorsMat_check_for_failed_motor(pMotor, throttle_thrust_best_plus_adj);
}

// check for failed motor
void MotorsMat_check_for_failed_motor(MotorsMat_HandleTypeDef *pMotor, float throttle_thrust_best)
{
    Motors_HandleTypeDef   *motors = (Motors_HandleTypeDef *)pMotor;
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    // record filtered and scaled thrust output for motor loss monitoring purposes
    float alpha = 1.0f / (1.0f + motors->_loop_rate * 0.5f);
    for (uint8_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            pMotor->_thrust_rpyt_out_filt[i] += alpha * (pMotor->_thrust_rpyt_out[i] - pMotor->_thrust_rpyt_out_filt[i]);
        }
    }

    float rpyt_high = 0.0f;
    float rpyt_sum = 0.0f;
    uint8_t number_motors = 0.0f;
    for (uint8_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            number_motors += 1;
            rpyt_sum += pMotor->_thrust_rpyt_out_filt[i];
            // record highest filtered thrust command
            if (pMotor->_thrust_rpyt_out_filt[i] > rpyt_high) {
                rpyt_high = pMotor->_thrust_rpyt_out_filt[i];
                // hold motor lost index constant while thrust boost is active
                if (!motors->_thrust_boost) {
                    pMotor->_motor_lost_index = i;
                }
            }
        }
    }

    float thrust_balance = 1.0f;
    if (rpyt_sum > 0.1f) {
        thrust_balance = rpyt_high * number_motors / rpyt_sum;
    }
    // ensure thrust balance does not activate for multirotors with less than 6 motors
    if (number_motors >= 6 && thrust_balance >= 1.5f && motors->_thrust_balanced) {
        motors->_thrust_balanced = false;
    }
    if (thrust_balance <= 1.25f && !motors->_thrust_balanced) {
        motors->_thrust_balanced = true;
    }

    // check to see if thrust boost is using more throttle than _throttle_thrust_max
    if ((motors_mc->_throttle_thrust_max * MotorsMC_get_compensation_gain(motors_mc) > throttle_thrust_best) && (rpyt_high < 0.9f) && motors->_thrust_balanced) {
        motors->_thrust_boost = false;
    }
}

// add_motor using raw roll, pitch, throttle and yaw factors
void MotorsMat_add_motor_raw(MotorsMat_HandleTypeDef *pMotor, int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order)
{
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    // ensure valid motor number is provided
    if (motor_num >= 0 && motor_num < GP_MOTORS_MAX_NUM_MOTORS) {

        // increment number of motors if this motor is being newly motor_enabled
        if (!motors_mc->motor_enabled[motor_num]) {
            motors_mc->motor_enabled[motor_num] = true;
        }

        // set roll, pitch, thottle factors and opposite motor (for stability patch)
        pMotor->_roll_factor[motor_num] = roll_fac;
        pMotor->_pitch_factor[motor_num] = pitch_fac;
        pMotor->_yaw_factor[motor_num] = yaw_fac;

        // set order that motor appears in test
        pMotor->_test_order[motor_num] = testing_order;

        // call parent class method
        Motors_add_motor_num(&(motors_mc->tMotors),motor_num);
    }
}

// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
void MotorsMat_normalise_rpy_factors(MotorsMat_HandleTypeDef *pMotor)
{
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    float roll_fac = 0.0f;
    float pitch_fac = 0.0f;
    float yaw_fac = 0.0f;

    // find maximum roll, pitch and yaw factors
    for (uint8_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            if (roll_fac < fabsf(pMotor->_roll_factor[i])) {
                roll_fac = fabsf(pMotor->_roll_factor[i]);
            }
            if (pitch_fac < fabsf(pMotor->_pitch_factor[i])) {
                pitch_fac = fabsf(pMotor->_pitch_factor[i]);
            }
            if (yaw_fac < fabsf(pMotor->_yaw_factor[i])) {
                yaw_fac = fabsf(pMotor->_yaw_factor[i]);
            }
        }
    }

    // scale factors back to -0.5 to +0.5 for each axis
    for (uint8_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            if (!math_flt_zero(roll_fac)) {
                pMotor->_roll_factor[i] = 0.5f * pMotor->_roll_factor[i] / roll_fac;
            }
            if (!math_flt_zero(pitch_fac)) {
                pMotor->_pitch_factor[i] = 0.5f * pMotor->_pitch_factor[i] / pitch_fac;
            }
            if (!math_flt_zero(yaw_fac)) {
                pMotor->_yaw_factor[i] = 0.5f * pMotor->_yaw_factor[i] / yaw_fac;
            }
        }
    }

}

// add_motor using just position and yaw_factor (or prop direction)
void MotorsMat_add_motor4(MotorsMat_HandleTypeDef *pMotor, int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order)
{
    MotorsMat_add_motor5(pMotor, motor_num, angle_degrees, angle_degrees, yaw_factor, testing_order);
}

// add_motor using separate roll and pitch factors (for asymmetrical frames) and prop direction
void MotorsMat_add_motor5(MotorsMat_HandleTypeDef *pMotor, int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order)
{
    MotorsMat_add_motor_raw(pMotor, 
                            motor_num,
                            cosf(Deg2Rad(roll_factor_in_degrees + 90)),
                            cosf(Deg2Rad(pitch_factor_in_degrees)),
                            yaw_factor,
                            testing_order);
}

// remove_motor
void MotorsMat_remove_motor(MotorsMat_HandleTypeDef *pMotor, int8_t motor_num)
{
    MotorsMC_HandleTypeDef *motors_mc = (MotorsMC_HandleTypeDef *)pMotor;

    // ensure valid motor number is provided
    if (motor_num >= 0 && motor_num < GP_MOTORS_MAX_NUM_MOTORS) {
        // disable the motor, set all factors to zero
        motors_mc->motor_enabled[motor_num] = false;
        pMotor->_roll_factor[motor_num] = 0;
        pMotor->_pitch_factor[motor_num] = 0;
        pMotor->_yaw_factor[motor_num] = 0;
    }
}

// configures the motors for the defined frame_class and frame_type
void MotorsMat_setup_motors(MotorsMat_HandleTypeDef *pMotor, motor_frame_class frame_class, motor_frame_type frame_type)
{
    Motors_HandleTypeDef *motors = (Motors_HandleTypeDef *)pMotor;

    // remove existing motors
    for (int8_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        MotorsMat_remove_motor(pMotor, i);
    }

    bool success = true;

    switch (frame_class) {

        case MOTOR_FRAME_QUAD:
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,  90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2, -90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,   0, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4, 180, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    break;
                case MOTOR_FRAME_TYPE_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    break;
                case MOTOR_FRAME_TYPE_BF_X:
                    // betaflight quad X order
                    // see: https://fpvfrenzy.com/betaflight-motor-order/
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
                    break;
                case MOTOR_FRAME_TYPE_BF_X_REV:
                    // betaflight quad X order, reversed motors
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    break;
                case MOTOR_FRAME_TYPE_DJI_X:
                    // DJI quad X order
                    // see https://forum44.djicdn.com/data/attachment/forum/201711/26/172348bppvtt1ot1nrtp5j.jpg
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    break;
                case MOTOR_FRAME_TYPE_CW_X:
                    // "clockwise X" motor order. Motors are ordered clockwise from front right
                    // matching test order
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    break;
                case MOTOR_FRAME_TYPE_V:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   45,  0.7981f,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2, -135,  1.0000f,  3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,  -45, -0.7981f,  4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  135, -1.0000f,  2);
                    break;
                case MOTOR_FRAME_TYPE_H:
                    // H frame set-up - same as X but motors spin in opposite directiSons
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    break;
                case MOTOR_FRAME_TYPE_VTAIL:
                    /*
                        Tested with: Lynxmotion Hunter Vtail 400
                        - inverted rear outward blowing motors (at a 40 degree angle)
                        - should also work with non-inverted rear outward blowing motors
                        - no roll in rear motors
                        - no yaw in front motors
                        - should fly like some mix between a tricopter and X Quadcopter

                        Roll control comes only from the front motors, Yaw control only from the rear motors.
                        Roll & Pitch factor is measured by the angle away from the top of the forward axis to each arm.

                        Note: if we want the front motors to help with yaw,
                            motors 1's yaw factor should be changed to sin(radians(40)).  Where "40" is the vtail angle
                            motors 3's yaw factor should be changed to -sin(radians(40))
                    */
                    MotorsMat_add_motor5(pMotor, GP_MOTORS_MOT_1, 60, 60, 0, 1);
                    MotorsMat_add_motor5(pMotor, GP_MOTORS_MOT_2, 0, -160, GP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
                    MotorsMat_add_motor5(pMotor, GP_MOTORS_MOT_3, -60, -60, 0, 4);
                    MotorsMat_add_motor5(pMotor, GP_MOTORS_MOT_4, 0, 160, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    break;
                case MOTOR_FRAME_TYPE_ATAIL:
                    /*
                        The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference:
                        - The Yaw factors are reversed, because the rear motors are facing different directions

                        With V-Shaped VTails, the props make a V-Shape when spinning, but with
                        A-Shaped VTails, the props make an A-Shape when spinning.
                        - Rear thrust on a V-Shaped V-Tail Quad is outward
                        - Rear thrust on an A-Shaped V-Tail Quad is inward

                        Still functions the same as the V-Shaped VTail mixing below:
                        - Yaw control is entirely in the rear motors
                        - Roll is is entirely in the front motors
                    */
                    MotorsMat_add_motor5(pMotor, GP_MOTORS_MOT_1, 60, 60, 0, 1);
                    MotorsMat_add_motor5(pMotor, GP_MOTORS_MOT_2, 0, -160, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor5(pMotor, GP_MOTORS_MOT_3, -60, -60, 0, 4);
                    MotorsMat_add_motor5(pMotor, GP_MOTORS_MOT_4, 0, 160, GP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
                    break;
                case MOTOR_FRAME_TYPE_PLUSREV:
                    // plus with reversed motor directions
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1, 90, GP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2, -90, GP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3, 0, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4, 180, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    break;
                default:
                    // quad frame class does not support this frame type
                    success = false;
                    break;
            }
            break;  // quad

        case MOTOR_FRAME_HEXA:
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   0, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2, 180, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,-120, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  60, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5, -60, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6, 120, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    break;
                case MOTOR_FRAME_TYPE_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,  90, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2, -90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3, -30, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4, 150, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,  30, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,-150, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    break;
                case MOTOR_FRAME_TYPE_H:
                    // H is same as X except middle motors are closer to center
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_1, -1.0f, 0.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_2, 1.0f, 0.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_3, 1.0f, 1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_4, -1.0f, -1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_5, -1.0f, 1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_6, 1.0f, -1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
                    break;
                case MOTOR_FRAME_TYPE_DJI_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   30, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,  -30, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,  -90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4, -150, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,  150, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,   90, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    break;
                case MOTOR_FRAME_TYPE_CW_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   30, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,   90, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,  150, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4, -150, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,  -90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,  -30, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);
                    break;
                default:
                    // hexa frame class does not support this frame type
                    success = false;
                    break;
            }
            break;

        case MOTOR_FRAME_OCTA:
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,    0,  GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,  180,  GP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,   45,  GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  135,  GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,  -45,  GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6, -135,  GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  -90,  GP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,   90,  GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    break;
                case MOTOR_FRAME_TYPE_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   22.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2, -157.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,   67.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  157.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,  -22.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6, -112.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  -67.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,  112.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    break;
                case MOTOR_FRAME_TYPE_V:
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_1,  0.83f,  0.34f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_2, -0.67f, -0.32f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_3,  0.67f, -0.32f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_4, -0.50f, -1.00f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_5,  1.00f,  1.00f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_6, -0.83f,  0.34f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_7, -1.00f,  1.00f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_8,  0.50f, -1.00f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
                    break;
                case MOTOR_FRAME_TYPE_H:
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_1, -1.0f,    1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_2,  1.0f,   -1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_3, -1.0f,  0.333f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_4, -1.0f,   -1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_5,  1.0f,    1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_6,  1.0f, -0.333f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_7,  1.0f,  0.333f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_8, -1.0f, -0.333f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    break;
                case MOTOR_FRAME_TYPE_I:
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_1, 0.333f, -1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_2, -0.333f,  1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_3,    1.0f, -1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_4,  0.333f,  1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_5, -0.333f, -1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_6,   -1.0f,  1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_7,   -1.0f, -1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_8,    1.0f,  1.0f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    break;
                case MOTOR_FRAME_TYPE_DJI_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   22.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,  -22.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  8);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,  -67.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4, -112.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5, -157.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,  157.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  112.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,   67.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    break;
                case MOTOR_FRAME_TYPE_CW_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   22.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,   67.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,  112.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  157.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5, -157.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6, -112.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  -67.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,  -22.5f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  8);
                    break;
                default:
                    // octa frame class does not support this frame type
                    success = false;
                    break;
            } // octa frame type
            break;

        case MOTOR_FRAME_OCTAQUAD:
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,    0, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,  -90, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,  180, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,   90, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,  -90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,    0, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,   90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,  180, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);
                    break;
                case MOTOR_FRAME_TYPE_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);
                    break;
                case MOTOR_FRAME_TYPE_V:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   45,  0.7981f, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,  -45, -0.7981f, 7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3, -135,  1.0000f, 5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  135, -1.0000f, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,  -45,  0.7981f, 8);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,   45, -0.7981f, 2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  135,  1.0000f, 4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8, -135, -1.0000f, 6);
                    break;
                case MOTOR_FRAME_TYPE_H:
                    // H frame set-up - same as X but motors spin in opposite directions
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  8);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
                    break;
                case MOTOR_FRAME_TYPE_CW_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,   45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,  135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6, -135, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,  -45, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  8);
                    break;
                default:
                    // octaquad frame class does not support this frame type
                    success = false;
                    break;
            }
            break;

        case MOTOR_FRAME_DODECAHEXA: {
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,     0, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);  // forward-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,     0, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);  // forward-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,    60, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);  // forward-right-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,    60, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);  // forward-right-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,   120, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);  // back-right-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,   120, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);  // back-right-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,   180, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  7);  // back-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,   180, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);  // back-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_9,  -120, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9);  // back-left-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_10, -120, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  10); // back-left-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_11,  -60, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  11); // forward-left-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_12,  -60, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12); // forward-left-bottom
                    break;
                case MOTOR_FRAME_TYPE_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,    30, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1); // forward-right-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,    30, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   2); // forward-right-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,    90, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   3); // right-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,    90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  4); // right-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,   150, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5); // back-right-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,   150, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   6); // back-right-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  -150, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   7); // back-left-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,  -150, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  8); // back-left-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_9,   -90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  9); // left-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_10,  -90, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  10); // left-bottom
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_11,  -30, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  11); // forward-left-top
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_12,  -30, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12); // forward-left-bottom
                    break;
                default:
                    // dodeca-hexa frame class does not support this frame type
                    success = false;
                    break;
            }}
            break;

        case MOTOR_FRAME_Y6:
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_Y6B:
                    // Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_1, -1.0f,  0.500f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_2, -1.0f,  0.500f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_3,  0.0f, -1.000f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_4,  0.0f, -1.000f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_5,  1.0f,  0.500f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_6,  1.0f,  0.500f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
                    break;
                case MOTOR_FRAME_TYPE_Y6F:
                    // Y6 motor layout for FireFlyY6
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_1,  0.0f, -1.000f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_2, -1.0f,  0.500f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_3,  1.0f,  0.500f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_4,  0.0f, -1.000f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_5, -1.0f,  0.500f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  2);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_6,  1.0f,  0.500f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  6);
                    break;
                default:
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_1, -1.0f,  0.666f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_2,  1.0f,  0.666f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  5);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_3,  1.0f,  0.666f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_4,  0.0f, -1.333f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  4);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_5, -1.0f,  0.666f, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);
                    MotorsMat_add_motor_raw(pMotor, GP_MOTORS_MOT_6,  0.0f, -1.333f, GP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
                    break;
            }
            break;

        case MOTOR_FRAME_DECA:
            switch (frame_type) {
                case MOTOR_FRAME_TYPE_PLUS:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,     0, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,    36, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,    72, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,   108, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,   144, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,   180, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  -144, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,  -108, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   8);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_9,   -72, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  9);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_10,  -36, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  10);
                    break;
                case MOTOR_FRAME_TYPE_X:
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_1,    18, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  1);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_2,    54, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   2);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_3,    90, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  3);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_4,   126, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   4);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_5,   162, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  5);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_6,  -162, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   6);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_7,  -126, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  7);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_8,   -90, GP_MOTORS_MATRIX_YAW_FACTOR_CW,   8);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_9,   -54, GP_MOTORS_MATRIX_YAW_FACTOR_CCW,  9);
                    MotorsMat_add_motor4(pMotor, GP_MOTORS_MOT_10,  -18, GP_MOTORS_MATRIX_YAW_FACTOR_CW,  10);
                    break;
                default:
                    // deca frame class does not support this frame type
                    success = false;
                    break;
            }
            break;

        default:
            // matrix doesn't support the configured class
            success = false;
            break;
    } // switch frame_class

    // normalise factors to magnitude 0.5
    MotorsMat_normalise_rpy_factors(pMotor);

    Motors_set_initialised_ok(motors,success);

}

/*
  disable the use of motor torque to control yaw. Used when an
  external mechanism such as vectoring is used for yaw control
*/
void MotorsMat_disable_yaw_torque(MotorsMat_HandleTypeDef *pMotor)
{
    for (uint8_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        pMotor->_yaw_factor[i] = 0;
    }
}

// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t MotorsMat_get_motor_mask(MotorsMat_HandleTypeDef *pMotor)
{
    Motors_HandleTypeDef* motors = (Motors_HandleTypeDef*)pMotor;
    MotorsMC_HandleTypeDef* mc_motors = (MotorsMC_HandleTypeDef*)pMotor;

    uint16_t motor_mask = 0;
    for (uint8_t i = 0; i < GP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (mc_motors->motor_enabled[i]) {
            motor_mask |= 1U << i;
        }
    }
    uint16_t mask = Motors_motor_mask_to_srv_channel_mask(motor_mask);

    // add parent's mask
    mask |= MotorsMC_get_motor_mask();

    return mask;
}

/*------------------------------------test------------------------------------*/


